#include <vector>
#include <string>
#include <iostream>
#include <sstream>
#include <fstream>
#include <map>
#include <set>
#include <utility>
#include <cmath>
#include <queue>
#include <iterator>
#include <algorithm>
#include "Vertex.h"
#include "Edge.h"
#include "Face.h"
#include "input.h"
#include "output.h"
#define INFILE "models/bunny.nf1k.m"
#define OUTFILE "bunny0.ply"
//#define INFILE "try.m"
//#define OUTFILE "try.ply"
#define NUM_STEINER 0
#define COLOR_SIZE 64
template <typename A>
void printVector(A first, A last)
{
    for (A it=first; it!=last; ++it)
        std::cout<<*(*it)<<"\t";
    std::cout<<std::endl;
}


void addSteinerPoints(std::vector<Vertex *>& vlist, std::vector<Edge *>& elist,int num,std::vector<Vertex *>& vSteinerList)
{
    int vid=vlist[vlist.size()-1]->id;
    int eid=elist[elist.size()-1]->id;
    for (std::vector<Edge *>::iterator it=elist.begin();it!=elist.end();++it)
    {
        float x=(*it)->v2->x;
        float y=(*it)->v2->y;
        float z=(*it)->v2->z;
        float x_unit=((*it)->v1->x-(*it)->v2->x)/(num+1);
        float y_unit=((*it)->v1->y-(*it)->v2->y)/(num+1);
        float z_unit=((*it)->v1->z-(*it)->v2->z)/(num+1);
        for (int i=1;i!=num+1;++i)
        {
            Vertex *steiner=new Vertex(vid++,x+i*x_unit,y+i*y_unit,z+i*z_unit);
            vSteinerList.push_back(steiner);
            (*it)->SteinerPoints.push_back(steiner);
        }
    }
}


void toAdjacencyList(std::vector<Vertex *>& vlist,std::vector<Edge *>& elist, std::vector<Face *>& flist, std::map<Vertex *,std::set<Vertex *> >& adjacencyList,int numSteinerPts)
{
    adjacencyList.clear();
    std::cout<<"ADDING VERTEX"<<std::endl;
    for (std::vector<Vertex *>::iterator it=vlist.begin();it!=vlist.end();++it)
    {
        std::set<Vertex *> s;
        adjacencyList.insert(std::pair<Vertex *,std::set<Vertex *> >(*it,s));
    }

    if (numSteinerPts>0)
    {
        std::cout<<"ADDING STEINER VERTEX"<<std::endl;
        std::vector<Vertex *> vSteinerList;
        addSteinerPoints(vlist, elist,numSteinerPts, vSteinerList);
        for (std::vector<Vertex *>::iterator it=vSteinerList.begin();it!=vSteinerList.end();++it)
        {
            std::set<Vertex *> s;
            adjacencyList.insert(std::pair<Vertex *,std::set<Vertex *> >(*it,s));
        }
        std::cout<<"ADDING STEINER EDGE"<<std::endl;
        for (std::vector<Face *>::iterator it=flist.begin();it!=flist.end();++it)
        {
            std::vector<Vertex *> stack;
            Edge *e1=(*it)->e1;
            Edge *e2=(*it)->e2;
            Edge *e3=(*it)->e3;
            Vertex *v1=e1->v1;
            Vertex *v2=e1->v2;
            Vertex *v3=e2->v1;
            if (v1==v3||v2==v3) v3=e2->v2;
            stack.push_back(v1);
            stack.push_back(v2);
            stack.push_back(v3);
            for (std::vector<Vertex *>::iterator iter=e1->SteinerPoints.begin();iter!=e1->SteinerPoints.end();++iter)
            {
                stack.push_back(*iter);
            }
            for (std::vector<Vertex *>::iterator iter=e2->SteinerPoints.begin();iter!=e2->SteinerPoints.end();++iter)
            {
                stack.push_back(*iter);
            }
            for (std::vector<Vertex *>::iterator iter=e3->SteinerPoints.begin();iter!=e3->SteinerPoints.end();++iter)
            {
                stack.push_back(*iter);
            }
            while (!stack.empty())
            {
                Vertex *top=stack.back();
                stack.pop_back();
                for (std::vector<Vertex *>::iterator iter=stack.begin();iter!=stack.end();++iter)
                {
                    adjacencyList[top].insert(*iter);
                    adjacencyList[*iter].insert(top);
                }
            }
        }
    }


    std::cout<<"ADDING EDGE"<<std::endl;
    for (std::vector<Edge *>::iterator it=elist.begin();it!=elist.end();++it)
    {
        adjacencyList[(*it)->v1].insert((*it)->v2);
        adjacencyList[(*it)->v2].insert((*it)->v1);
    }
}
class vertexCompare
{
    std::map<Vertex *,std::pair<double, Vertex *> >* lookup;
    public:
    vertexCompare(std::map<Vertex *,std::pair<double, Vertex *> >& lookup)
    {
        this->lookup=&lookup;
    }

    bool operator()(Vertex *v1, Vertex *v2)
    {
        return (*lookup)[v1].first>(*lookup)[v2].first;
    }
};

double distance(Vertex *u,Vertex *v)
{
    return sqrt(pow(u->x-v->x,2)+pow(u->y-v->y,2)+pow(u->z-v->z,2));
}
void dijkstra(std::map<Vertex *,std::set<Vertex *> >& adjacencyList, Vertex& s, std::map<Vertex *, std::pair<double, Vertex *> >& outcome)
{
    std::pair<double, Vertex *> value(INFINITY,NULL);
    for(std::map<Vertex *,std::set<Vertex *> >::iterator it=adjacencyList.begin();it!=adjacencyList.end();++it)
    {
        outcome.insert(std::pair<Vertex *, std::pair<double,Vertex *> >(it->first,value));
    }

    outcome[&s].first=0;
    std::priority_queue<Vertex *,std::vector<Vertex *>,vertexCompare> q((vertexCompare(outcome)));

    for(std::map<Vertex *,std::set<Vertex *> >::iterator it=adjacencyList.begin();it!=adjacencyList.end();++it)
    {
        q.push(it->first);
    }

    while(!q.empty())
    {
        if (q.size()%1000==0) std::cout<<q.size()<<std::endl;
        Vertex *u=q.top();
        q.pop();
        for(std::set<Vertex *>::iterator it=adjacencyList[u].begin();it!=adjacencyList[u].end();++it)
        {
            if(outcome[u].first+distance(u,*it)<outcome[*it].first)
            {
                outcome[*it].first=outcome[u].first+distance(u,*it);
                outcome[*it].second=u;

                //updating the queue
                std::priority_queue<Vertex *,std::vector<Vertex *>,vertexCompare> temp((vertexCompare(outcome)));
                while(!q.empty())
                {
                    Vertex *aVertex=q.top();
                    q.pop();
                    temp.push(aVertex);
                }
                q=temp;
            }
        }
    }
}
int main()
{
    std::vector<Vertex *> vlist;
    std::vector<Edge *> elist;
    std::vector<Face *> flist;
    std::cout<<"READING DATA"<<std::endl;
    readData(INFILE,vlist,elist,flist);
    printVector(vlist.begin(),vlist.end());
    printVector(elist.begin(),elist.end());
    printVector(flist.begin(),flist.end());


    std::map<Vertex *,std::set<Vertex *> > adjacencyList;
    std::cout<<"CONVERTING DATA"<<std::endl;
    toAdjacencyList(vlist,elist,flist,adjacencyList,NUM_STEINER);

    std::map<Vertex *, std::pair<double, Vertex *> > outcome;
    std::cout<<"CALCULATING SHORTEST PATH"<<std::endl;
    dijkstra(adjacencyList,*vlist[0],outcome);





    /*
    for (std::vector<Vertex>::iterator it=vlist.begin();it!=vlist.end();++it)
    {
        std::cout<<(*it)<<"\t"<<outcome[&(*it)].first<<std::endl;
    }
    */
    std::cout<<"CREATING IMAGE"<<std::endl;
    writeToPly(OUTFILE,vlist,flist,outcome);
    /*
    for (std::map<Vertex *,std::set<Vertex *> >::iterator it=adjacencyList.begin();it!=adjacencyList.end();++it)
    {
        std::cout<<(*(it->first))<<"|";
        for (std::set<Vertex *>::iterator it2=(it->second).begin();it2!=(it->second).end();++it2)
        {
            std::cout<<"->"<<(**it2);
        }
        std::cout<<std::endl;
    }
    */
    for (std::vector<Vertex *>::iterator it=vlist.begin();it!=vlist.end();++it)
        delete (*it);
    for (std::vector<Edge *>::iterator it=elist.begin();it!=elist.end();++it)
        delete (*it);
    for (std::vector<Face *>::iterator it=flist.begin();it!=flist.end();++it)
        delete (*it);
    return 0;
}
